#include "PatrolState.h"

CPatrolState::CPatrolState()
: m_tCurrentGoal(m_tRoute.end())
{
}

CPatrolState::~CPatrolState()
{
}

CPatrolState* CPatrolState::Instance()
{
	static CPatrolState instance;
	return &instance;
}

void CPatrolState::SetRoute(const std::vector<sf::Vector2f>& points)
{
	m_tRoute = points;
	m_tCurrentGoal = m_tRoute.begin();
}

void CPatrolState::OnEnter(CCharacter* entity)
{
}

void CPatrolState::OnExit(CCharacter* entity)
{
	m_tRoute.clear();
}

void CPatrolState::OnExecute(CCharacter* entity, float dt)
{
	if (m_tRoute.empty())
	{
		return;
	}

	const sf::Vector2f& entityPos = entity->GetPosition();
	sf::Vector2f distanceVec = (*m_tCurrentGoal) - entityPos;
	float distance = sqrt((distanceVec.x * distanceVec.x) + (distanceVec.y * distanceVec.y));

	const float kArriveDist = 0.5f;

	if (distance < kArriveDist)
	{
		m_tCurrentGoal++;
		if (m_tCurrentGoal == m_tRoute.end())
		{
			m_tCurrentGoal = m_tRoute.begin();
			OnExecute(entity, dt); // Because recursion is always win
		}
	}
	else
	{
		// normalise the distance vector to get a direction
		distanceVec.x /= distance;
		distanceVec.y /= distance;

		// face the way we're going
		entity->SetAngle(atan2(distanceVec.y, distanceVec.x));

		float speedByDT = entity->GetMovementSpeed() * dt;
		distanceVec.x *= speedByDT;
		distanceVec.y *= speedByDT;

		entity->SetPosition(entityPos + distanceVec);
	}
}
